﻿using Accord.Math;
using AMEE.MashineLearning.Geometry;
using AMEE.PlaneDetection.Kinect;
using Microsoft.Kinect;
using System;
using System.Threading.Tasks;

namespace AMEE.PlaneDetection
{
    /// <summary>
    /// Event Arguments of an PlaneDetected event.
    /// </summary>
    public class PlaneDetectedEventArgs : EventArgs
    {
        /// <summary>
        /// Gets or sets the inlier's indexes.<para>
        /// That Indexes of the <c>PointCloud</c> array, whose points belongs to the <c>Plane</c>.</para>
        /// </summary>
        /// <value>
        /// The inlier's indexes.
        /// </value>
        public int[] Inliers { get; set; }

        /// <summary>
        /// Gets or sets the kinect's point cloud.
        /// </summary>
        /// <value>
        /// The point cloud.
        /// </value>
        public CameraSpacePoint[] PointCloud { get; set; }

        /// <summary>
        /// Gets or sets the plane definition found by ransac.
        /// </summary>
        /// <value>
        /// The plane.
        /// </value>
        public Plane Plane { get; set; }
    }



    public class PlaneFinder
    {
        #region Fields
        K4Wv2PointCloud _ptCloudReader;
        #endregion


        #region Properties & Events

        /// <summary>
        /// Occurs when plane was detected.
        /// </summary>
        public event EventHandler<PlaneDetectedEventArgs> PlaneDetected;

        public event EventHandler NoPlaneFound;

        /// <summary>
        /// Gets or sets the minimum distance between a data point and
        /// the model used to decide whether the point is an inlier or not.
        /// </summary>
        /// <value>
        /// The ransac threshold.
        /// </value>
        public double RansacThreshold { get; set; }

        /// <summary>
        /// Gets or sets the probability of obtaining a random
        /// sample of the input points that contains no outliers.
        /// Default is 0.99.
        /// <para>
        /// The probability is a minimum probability of finnding at least one good 
        /// set of observations in N trials. It lies usually between 0.90 and 0.99.
        /// </para>
        /// </summary>
        /// <value>
        /// The ransac probability.
        /// </value>
        public double RansacProbability { get; set; }

        /// <summary>
        /// Gets or sets the maximum frame rate for point clouds (frames per second).
        /// </summary>
        /// <value>
        /// The frame rate. Set to 0 (default), to disable a maximum frame rate.
        /// </value>
        public double FrameRate {
            get;
            set;
        }

        #endregion


        #region Constructor(s)


        #endregion


        #region Methods

        static int GCD(int a, int b) {
            return b == 0 ? a : GCD(b, a % b); 
        }

        /// <summary>
        /// One shot scan of point cloud.<para>
        /// If found a plane, throws a <c>PlaneDetected</c> event with the result.</para>
        /// </summary>
        public void ScanOnce() {
            initPointCloudReader();
            _ptCloudReader.PointCloudReady += scanOnce_PointCloudReady;
            _ptCloudReader.Continue();
        }

        /// <summary>
        /// Asynchronous one shot scan of point cloud to retrieve a <c>Plane</c> .
        /// <para>Set FrameRate to slow down the scanner (to skip a couple of PointCloud frames).</para>
        /// </summary>
        /// <returns>awaitable PlaneDetectedEventArgs. null, if no plane detected<para>
        /// Use: <c>PlaneDetectedEventArgs pdea = await ScanOnceAsync();</c></para>
        /// </returns>
        public async Task<PlaneDetectedEventArgs> ScanOnceAsync() {  // alternative Umsetzung: Asynchrone Methode mit Callback
            initPointCloudReader();
            // Wenn PCL-Event kommt, setze das EventArgument als Task-Result
            var tcs = new TaskCompletionSource<PointCloudReadyEventArg>();
            EventHandler<PointCloudReadyEventArg> evHandler = (sender, pclEventArg) => {
                tcs.SetResult(pclEventArg);
            };

            // Wenn Task-Result gesetzt, fahre fort mit plane detection
            var task = tcs.Task.ContinueWith<PlaneDetectedEventArgs>(getContinuationFunction(evHandler));
            _ptCloudReader.PointCloudReady += evHandler;
            _ptCloudReader.Continue();
            //tcs.Task.Start(); // nötig?
            var resu = await task;
            return resu;
        }

        public async Task<PlaneDetectedEventArgs[]> ScanOnceAsync(int rows, int cols) {  // alternative Umsetzung: Asynchrone Methode mit Callback
            initPointCloudReader();
            // Wenn PCL-Event kommt, setze das EventArgument als Task-Result
            var tcs = new TaskCompletionSource<PointCloudReadyEventArg>();
            EventHandler<PointCloudReadyEventArg> evHandler = (sender, pclEventArg) => {
                tcs.SetResult(pclEventArg);
            };

            // Wenn Task-Result gesetzt, fahre fort mit plane detection
            var task = tcs.Task.ContinueWith<PlaneDetectedEventArgs[]>(getMultiPlaneContinuationFunction(evHandler, rows, cols));
            _ptCloudReader.PointCloudReady += evHandler;
            _ptCloudReader.Continue();
            //tcs.Task.Start(); // nötig?
            var resu = await task;
            return resu;
        }

        /// <summary>
        /// Gets the continuation function.
        /// </summary>
        /// <param name="evHandler">The event Handler attached to <c>K4Wv2PointCloud</c>.</param>
        /// <returns>continuation function.</returns>
        /// <remarks>
        /// Sobald <c>PointCloudReadyEventArg</c> geliefert wird, erst PCL-EventHandler abmelden und
        /// <c>K4Wv2PointCloud</c> pausieren. Dann <c>RansacPlane</c> anwenden und das Ergebnis (ggf. null)
        /// zurückgeben.
        /// </remarks>
        private Func<Task<PointCloudReadyEventArg>, PlaneDetectedEventArgs> getContinuationFunction(EventHandler<PointCloudReadyEventArg> evHandler) {
            return tsk1 => {
                _ptCloudReader.PointCloudReady -= evHandler; // detach Handler
                _ptCloudReader.Pause(); // pausieren
                var k4wPoints = tsk1.Result.PointCloud;
                var resu = estimatePlane(k4wPoints);
                if (resu.Inliers == null) {
                    return null;
                }
                return resu;
            };
        }

        /// <summary>
        /// Gets the multi plane continuation function.
        /// </summary>
        /// <param name="evHandler">The event Handler attached to <c>K4Wv2PointCloud</c>.</param>
        /// <param name="tiles">The number of tiles the orig. point cloud will splitted to.</param>
        /// <returns>continuation function</returns>
        /// <remarks>
        /// Sobald <c>PointCloudReadyEventArg</c> geliefert wird, erst PCL-EventHandler abmelden und
        /// <c>K4Wv2PointCloud</c> pausieren. Dann Punktwolke aufteilen und jeweils
        /// <c>RansacPlane</c> anwenden und die Ergebnisse zurückgeben. Wenn in einem
        /// Kachelstück keine Fläche gefunden wurde, ist das zugehörige Inliers-Array null und die
        /// Plane ist die Standardplane (0).
        /// </remarks>
        private Func<Task<PointCloudReadyEventArg>, PlaneDetectedEventArgs[]> getMultiPlaneContinuationFunction(EventHandler<PointCloudReadyEventArg> evHandler, int rows, int cols) {
            return tsk1 => {
                _ptCloudReader.PointCloudReady -= evHandler; // detach Handler
                _ptCloudReader.Pause(); // pausieren
                var k4wPoints = tsk1.Result.PointCloud;

                var results = new PlaneDetectedEventArgs[rows*cols];
                CameraSpacePoint[][] cloudTiles = getTiles(rows, cols, k4wPoints);
                // 3) Rufe für jedes Subarray planeRansac auf und speichere das Ergebnis.
                for (int t = 0; t < results.Length; t++) {
                    var result_t = estimatePlane(cloudTiles[t]);
                    results[t] = result_t;
                }
                return results;
            };
        }

        /// <summary>
        /// Estimates the plane from the specified point cloud with <c>PlaneRansac</c>.
        /// </summary>
        /// <param name="cloud">the point cloud (not null)</param>
        /// <returns>
        /// <c>PlaneDetectedEventArgs</c> with plane found and inlier indices.<para>
        /// If no plane was found, the Inliers property will be null an the plane the default.</para>
        /// </returns>
        private PlaneDetectedEventArgs estimatePlane(CameraSpacePoint[] cloud) {
            var result = new PlaneDetectedEventArgs { PointCloud = cloud };
            Point3[] theCloud = cloud.ToAccord();
            var plRansac = new RansacPlane(RansacThreshold, RansacProbability);
            var planeAccord = plRansac.Estimate(theCloud);
            if (planeAccord != null) {
                var plane = new Plane {
                    A = planeAccord.A,
                    B = planeAccord.B,
                    C = planeAccord.C,
                    Offset = planeAccord.Offset
                };
                result.Inliers = plRansac.Inliers;
                result.Plane = plane;
            }
            // else {
            //   result_t.Inliers bleibt null und .Plane bleibt default
            // }
            return result;
        }

        /// <summary>
        /// Liefert ein Array mit '<c>tiles</c>' vielen Submatritzen der originalen Punktwolke.
        /// </summary>
        /// <param name="rows">Anzahl der vertikalen Unterteilungen (Zonen).</param>
        /// <param name="cols">Anzahl der horizontalen Unterteilungen (Zonen).</param>
        /// <param name="k4wPoints">Die Punktwolke von der Kinect</param>
        /// <returns>Array of sub-matrices by tiling the specified point cloud matrix</returns>
        private CameraSpacePoint[][] getTiles(int rows, int cols, CameraSpacePoint[] k4wPoints) {
            // 1) Wieviele unterteilungen des DepthFrames?
            int rowsOfSub = rows;
            int colsOfSub = cols;
            //
            //int rowsOfSub = 1;
            //int colsOfSub = tiles;
            //// Aufteilung der Blöcke.
            //// Daher such die gröte Zahl, durch die sich 'tiles' teilen lässt
            //for (int div = tiles / 2; div > 1; div--) {
            //    int gcd = PlaneFinder.GCD(tiles, div);
            //    if (gcd > 1) {
            //        colsOfSub = gcd;
            //        break;
            //    }
            //}
            //if (colsOfSub != tiles) {
            //    rowsOfSub = tiles / colsOfSub;
            //}

            int k4wWidth = _ptCloudReader.PixelWidth;
            int k4wHeight = _ptCloudReader.PixelHeight;
            int widthNew = k4wWidth - (k4wWidth % colsOfSub);     // #Indizes je Spalte
            int heightNew = k4wHeight - (k4wHeight % rowsOfSub);  // #Zeilen in Orig.Cloud je Zeile
            int tileWidth = widthNew / colsOfSub;    // #Spalten Je Submatrix
            int tileHeight = heightNew / rowsOfSub;  // #Zeilen je Submatrix
            int tileSize = tileWidth * tileHeight;

            // (Erst Im 2. EntwSchritt: wie viele Unterteilungen in der Tiefe?

            // 2) Erzeuge SubArrays und kopiere(?) Daten (ggf. Parallel.For)


            CameraSpacePoint[][] clouds = new CameraSpacePoint[rows*cols][]; // Array of sub-matrices (tiles)
            for (int i = 0; i < clouds.Length; i++) {
                clouds[i] = new CameraSpacePoint[tileSize];
            }
            // Erstelle #blocks SubArrays
            Parallel.For(0, k4wPoints.Length, (i, loopState) => {
                int k4wRow = i / k4wWidth; // Zeile der original Matrix
                int k4wCol = i % k4wWidth; // Spalte der Originalmatrix
                // ggf. letzte Zeilen / Spalten der Originaldaten abschneiten
                if (k4wCol < widthNew && k4wRow < heightNew) {
                    // Ermittle SubMatrix Nummer
                    int cloudsRowNum = k4wRow / tileHeight;
                    int cloudesColNum = k4wCol / tileWidth;
                    int tileNum = (cloudsRowNum * colsOfSub) + cloudesColNum;
                    var subMatrix = clouds[tileNum];

                    // Ermittle Index innerhalb der Submatrix (tile)
                    int tileCol = k4wCol % tileWidth;
                    int tileRow = k4wRow % tileHeight;
                    int tileIdx = (tileRow * tileWidth) + tileCol;// (subRow * k4wWidth) + subCol;

                    clouds[tileNum][tileIdx].X = k4wPoints[i].X;
                    clouds[tileNum][tileIdx].Y = k4wPoints[i].Y;
                    clouds[tileNum][tileIdx].Z = k4wPoints[i].Z;
                }
            });
            return clouds;
        }

        /// <summary>
        /// Initializes the point cloud reader.<para>
        /// Erzeugt ggf. neue <c>K4Wv2PointCloud</c>, setzt FrameRate,
        /// initialisiert und pausiert <c>K4Wv2PointCloud</c>.
        /// </para>
        /// </summary>
        private void initPointCloudReader() {
            if (_ptCloudReader == null) {
                _ptCloudReader = new K4Wv2PointCloud();
                _ptCloudReader.Initialize();
            }
            _ptCloudReader.Pause();
            _ptCloudReader.FrameRate = this.FrameRate;
        }


        /// <summary>
        /// 
        /// </summary>
        /// <param name="sender">The sender.</param>
        /// <param name="e">The eventArgs</param>
        void scanOnce_PointCloudReady(object sender, PointCloudReadyEventArg e) {
            var k4wPoints = e.PointCloud;
            Point3[] theCloud = k4wPoints.ToAccord();

            var task = new Task(pcl => {
                // find plane in point cloud:
                Point3[] points = (Point3[])pcl;
                var plRansac = new RansacPlane(RansacThreshold, RansacProbability);
                var planeAccord = plRansac.Estimate(points);
                if (planeAccord != null) {
                    // Throw PlaneDetected event and pass on arguments:
                    if (PlaneDetected != null) {
                        var plane = new Plane {
                            A = planeAccord.A,
                            B = planeAccord.B,
                            C = planeAccord.C,
                            Offset = planeAccord.Offset
                        };
                        PlaneDetected(this, new PlaneDetectedEventArgs {
                            PointCloud = k4wPoints,
                            Inliers = plRansac.Inliers,
                            Plane = plane
                        });
                    }
                }
                // else: Keine Fläche gefunden, warum auch immer -> kein Event werfen
                else {
                    if (NoPlaneFound != null) {
                        NoPlaneFound(this, null);
                    }
                }

                // detach this PointCloudReady Handler and pause pcl reading
                _ptCloudReader.PointCloudReady -= scanOnce_PointCloudReady;
                _ptCloudReader.Pause(); // pausieren
            }, theCloud);
            task.Start();
        }


        /// <summary>
        /// Stops this instance and releases the kinect ressource.
        /// </summary>
        public void Stop() {
            if (_ptCloudReader != null) {
                _ptCloudReader.Stop();
            }
        }

        #endregion
    }
}
